#include "FPGrabber.h"

#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/io.hpp>
#include <GL/glut.h>
#include <iostream>
#include <fstream>
#include <stdexcept>
#include "Util.h"

using namespace boost::numeric::ublas;

FPGrabber::FPGrabber(boost::numeric::ublas::vector<float> pos,
					 boost::numeric::ublas::vector<float> ori,
					 boost::numeric::ublas::vector<float> up) : rstPos(pos), rstOri(ori), rstUp(up)
{
	pos = vector<float>(3);
	ori = vector<float>(3);
	up = vector<float>(3);

	lastX = lastY = 1;

	posStates = std::vector< vector<float> >();
	oriStates = std::vector< vector<float> >();
	upStates = std::vector< vector<float> >();
	
	memset(keyDown, 0, sizeof(keyDown));
	reset();
}

void FPGrabber::transform() 
{
	if(keyDown['w'])
		forward();
	if(keyDown['s'])
		backward();
	if(keyDown['a'])
		left();
	if(keyDown['d'])
		right();
	gluLookAt(pos(0), pos(1), pos(2), pos(0)+ori(0), pos(1)+ori(1), pos(2)+ori(2), up(0), up(1), up(2));
}

void FPGrabber::reset() 
{
	pos = rstPos;
	up = rstUp;
	ori = rstOri;
}

void FPGrabber::handleKey(unsigned char key) 
{
	keyDown[key] = 1;

	/*
	switch(key) {
  case 'w':
	  forward();
	  break;
  case 's':
	  backward();
	  break;
  case 'a':
	  left();
	  break;
  case 'd':
	  right();
	  break;
	}
	*/
	glutPostRedisplay();
}

void FPGrabber::handleKeyUp(unsigned char key){

	keyDown[key] = 0;
	glutPostRedisplay();
}

void FPGrabber::rotate(int key, float delta) 
{
	float angle;

	if(key==GLUT_KEY_LEFT) {
		angle = delta;
	}
	else if(key==GLUT_KEY_RIGHT){
		angle = -delta;
	}
	else if(key==GLUT_KEY_UP) {
		angle = delta;
	}
	else if(key==GLUT_KEY_DOWN) {
		angle = -delta;
	}
	else if(key==GLUT_KEY_PAGE_DOWN) {
		angle = delta;
	}
	else {
		angle = -delta;
	}

	using namespace boost::numeric::ublas;

	vector<float> cross = cgmath::cross(ori,up);

	cross = cross/norm_2(cross);


	matrix<float> rot;
	if(key==GLUT_KEY_LEFT||
		key==GLUT_KEY_RIGHT) {

			rot = cgmath::rotMatrix(up,
				cross,
				ori,
				angle);
			ori = prod(rot,ori);
			up = cgmath::cross(cross,ori);
	}
	else if(key==GLUT_KEY_UP||
		key==GLUT_KEY_DOWN) {
			rot = cgmath::rotMatrix(cross,
				ori,
				up,
				angle);
			ori = prod(rot,ori);
			up = cgmath::cross(cross,ori);
	}
	else {
		rot = cgmath::rotMatrix(ori,
			up,
			cross,
			angle);
		up = prod(rot,up);
		ori = cgmath::cross(up,cross);
	}




	up = up/norm_2(up);
	ori = ori/norm_2(ori);

	//   std::cout << "up " << up << " ori " << ori << std::endl;
	//   std::cout << "|up| " << norm_2(up) << " |ori| " << norm_2(ori) << std::endl;
	//  std::cout << "prod " << inner_prod(up,ori) << std::endl;
}
void FPGrabber::forward() 
{
	pos+=ori*0.5;
}

void FPGrabber::backward() 
{
	pos-=ori*0.5;
}

void FPGrabber::left() 
{
	using namespace boost::numeric::ublas;

	vector<float> add = cgmath::cross(up,ori);

	pos+=add*1.0f;
}

void FPGrabber::right() 
{
	using namespace boost::numeric::ublas;

	vector<float> add = cgmath::cross(up,ori);

	pos-=add*1.0f;
}


void FPGrabber::handleMouse(int button, int state,int x, int y) 
{
	if(GLUT_LEFT_BUTTON==button&&
		state==GLUT_DOWN) {
		float diffX = ((float)x -lastX)/-5.0f;
		float diffY = ((float)y -lastY)/500.0f;

		using namespace boost::numeric::ublas;

		matrix<float> rot;

		rot = cgmath::rotYMat(
			diffX);
		ori = prod(rot,ori);
		up = prod(rot,up);

		vector<float> cross = cgmath::cross(ori,up);
		cross = cross/norm_2(cross);
		rot = cgmath::rotMatrix(cross,
			ori,
			up,
			diffY);
		ori = prod(rot,ori);
		up = cgmath::cross(cross,ori);
		up = up/norm_2(up);
		ori = ori/norm_2(ori);


		up = up/norm_2(up);
		ori = ori/norm_2(ori);
	}

	lastX = x;
	lastY = y;
}

void FPGrabber::handleSpecialKey(int key, int mod) 
{
	if(key==GLUT_KEY_UP||
		key==GLUT_KEY_DOWN||
		key==GLUT_KEY_RIGHT||
		key==GLUT_KEY_LEFT||
		key==GLUT_KEY_PAGE_DOWN||
		key==GLUT_KEY_PAGE_UP) {
			rotate(key);
	}
	else if(key==GLUT_KEY_F12) {
		posStates.push_back(pos);
		oriStates.push_back(ori);
		upStates.push_back(up);
	}
	else if(key==GLUT_KEY_F11) {
		std::vector< vector<float> >::iterator itPos = posStates.begin();
		std::vector< vector<float> >::iterator itOri = oriStates.begin();
		std::vector< vector<float> >::iterator itUp =  upStates.begin();
		for(; itPos != posStates.end() &&
			itOri != oriStates.end() &&
			itUp != upStates.end();) {
				std::cout << "pos: " << (*itPos++) << " ori: " << (*itOri++) << " up: " << (*itUp++) << std::endl;
		}
	}
	else if(key==GLUT_KEY_F10) {
		std::cout << "pos: " << pos << std::endl;
	}

	else if(key==GLUT_KEY_F9) {
		write("default.key");
	}
	else if(key==GLUT_KEY_F10) {
		read("default.key");
	}
}

boost::numeric::ublas::vector<float> & FPGrabber::getPos()
{
	return pos;
}

boost::numeric::ublas::vector<float> & FPGrabber::getOri()
{
	return ori;
}

std::vector< boost::numeric::ublas::vector<float> > & FPGrabber::getPosStates()
{
	return posStates;
}

std::vector< boost::numeric::ublas::vector<float> > & FPGrabber::getOriStates() 
{
	return oriStates;
}

std::vector< boost::numeric::ublas::vector<float> > & FPGrabber::getUpStates() 
{
	return upStates;
}

void FPGrabber::write(std::string filename) 
{
	std::ofstream ostr(filename.c_str());
	if (!ostr.is_open()) { 
		throw std::runtime_error("couldn't open " + filename);
	}

	// write lenght
	ostr << posStates.size();

	// write positions
	for(std::vector< boost::numeric::ublas::vector<float> >::iterator it = posStates.begin();
		it != posStates.end(); it++) {

			ostr << (*it);
	}

	// write orientations
	for(std::vector< boost::numeric::ublas::vector<float> >::iterator it = oriStates.begin();
		it != oriStates.end(); it++) {

			ostr << (*it);
	}

	// write up
	for(std::vector< boost::numeric::ublas::vector<float> >::iterator it = upStates.begin();
		it != upStates.end(); it++) {

			ostr << (*it);
	}
	ostr << std::endl;
}

void FPGrabber::read(std::string filename) 
{
	std::ifstream istr(filename.c_str());
	if (!istr.is_open()) { 
		throw std::runtime_error("couldn't open " + filename);
	}

	posStates.clear();
	oriStates.clear();
	upStates.clear();


	unsigned int n;
	istr >> n;

	boost::numeric::ublas::vector<float> tmp;

	// read positions
	for(unsigned int i = 0; i < n; i++) {
		istr >> tmp;
		posStates.push_back(tmp);
	}

	// read orientations
	for(unsigned int i = 0; i < n; i++) {
		istr >> tmp;
		oriStates.push_back(tmp);
	}

	// read up
	for(unsigned int i = 0; i < n; i++) {
		istr >> tmp;
		upStates.push_back(tmp);
	}
}

int FPGrabber::isKeyDown(unsigned char c){
	return keyDown[c];
}